#include <algorithm>
#include <Geex/basics/assert.h>
#include "particle_packer.h"

namespace Particle_packing
{
	Particle_packer* Particle_packer::instance_ = 0 ;
	
	Particle_packer::Particle_packer(const std::string& input_fn, bool circle_union) : rand_gen(std::time(0))
	{
		gx_assert(instance_ == 0) ;
		instance_ = this ;
		
		if (!circle_union)
		{
			read_circles(input_fn, std::back_inserter(circles));
			std::cout<<circles.size()<<" circles have been input.\n";
		}
		else
		{
			read_circle_unions(input_fn, std::back_inserter(circle_unions));
			std::cout<<circle_unions.size()<<" circle unions have been input.\n";
		}
		
		rbox = new Rectangular_box(-1.0, -1.0, 1.0, 1.0);
		//rbox = new Rectangular_box(0.0, 0.0, 168.0, 171.0);

		min_energy = std::numeric_limits<double>::max();
		best_config.reserve(circles.size());

		std::srand(std::time(0));
	}
	Particle_packer::~Particle_packer() 
	{
		delete rbox;

		instance_ = 0 ; 
	}

	void Particle_packer::pack(unsigned int nb_trials)
	{

		for (unsigned int i = 0; i < nb_trials; i++)
		{
			double e = energy_minimize();
			if (e < min_energy)
			{
				min_energy = e;
				best_config.assign(circles.begin(), circles.end());
			}
			glut_viewer_redraw();
		}
		std::cout << "Minimal energy found = "<<min_energy<<std::endl;
		circles.assign(best_config.begin(), best_config.end());
		glut_viewer_redraw();
	}
	
	double Particle_packer::energy_minimize()
	{
		static double opti_time = 0.0;
		static unsigned int opti_nb = 0;

		CGAL::Timer timer;
		if (!circles.empty())
		{
			System_potential_energy *system_energy = new Discrete_circle_system(circles, *rbox);
			double *trans = new double[circles.size()*3];
			//std::fill(trans, trans + circles.size()*4, 0.0);
			int res = system_energy->optimize(trans, true);
			double e = std::numeric_limits<double>::max();	
	
			if (res >= -400)
			//if (res-1000 >= 0 && res-1000<= 13 || res == -1004)
			{
				// after query, we do update
				double ge = system_energy->gravity_energy_at(trans);
				e = system_energy->system_energy_at(trans);
				for (unsigned int i = 0; i < circles.size(); i++)
				{
					//circles[i].translate(trans[i*2], trans[2*i+1]);	
					double c = std::cos(trans[3*i]), s = std::sin(trans[3*i]);
					//double c = trans[4*i], s = trans[4*i+1];
					double tx = trans[3*i+1], ty = trans[3*i+2];
					Parameter::Transformation_2 t(c, -s, tx, s, c, ty);
					circles[i] = Particle_circle(t(circles[i].center()), circles[i].squared_radius());
				}
				std::cout<<"Potential energy = "<<e/*<<", gravity energy = "<<ge<<", LJ energy = "<<e-ge*/<<std::endl;
			}
			else
			{
				std::cout<<"Minimization failed, error code = "<<res<<std::endl;
			}
			delete[] trans;
			delete system_energy;
			return e;
		}
		else
		{
			System_potential_energy *system_energy = new Particle_packing::Circle_union_system(circle_unions, *rbox);
			double *trans = new double[circle_unions.size()*3];
			std::fill(trans, trans + circle_unions.size()*3, 0.0);

			timer.start();

			int res = system_energy->optimize(trans, false);
			double e = std::numeric_limits<double>::max();	
	
			if (res > -200)
			{
				for (unsigned int i = 0; i < circle_unions.size(); i++)
				{
					//std::cout<<'['<<trans[i*3]<<','<<trans[3*i+1]<<','<<trans[3*i+2]<<"], ";
					circle_unions[i].transform(trans[i*3], trans[3*i+1], trans[3*i+2]);	
				}
				//std::cout<<"\nPotential energy = "<<e<<", gravity energy = "<<ge<<", LJ energy = "<<e-ge<<std::endl;
#if 1
				std::cout<<"Try to eliminate overlap...\n";
				std::fill(trans, trans + circle_unions.size()*3, 0.0);
				res = system_energy->optimize(trans, true);
				if (res > -200)
				{
					// after query, we do update
					double ge = system_energy->gravity_energy_at(trans);
					e = system_energy->system_energy_at(trans);
					std::cout<<"Potential energy = "<<e<<", gravity energy = "<<ge<<", LJ energy = "<<e-ge<<std::endl;
					for (unsigned int i = 0; i < circle_unions.size(); i++)
					{
						circle_unions[i].transform(trans[i*3], trans[3*i+1], trans[3*i+2]);	
					}
				}
				else
				{
					std::cout<<"Failed to eliminate overlap, with error code = "<<res<<std::endl;
				}
#endif
			}
			else
			{
				std::cout<<"Minimization failed, error code = "<<res<<std::endl;
			}

			timer.stop();
			opti_time += timer.time();
			opti_nb++;
			std::cout<<"Average optimization time = "<<opti_time/opti_nb<<std::endl;
			delete[] trans;
			delete system_energy;
			return e;
		}

		
	}
	void Particle_packer::perturb()
	{
		boost::random::uniform_real_distribution<double> uni_distr(0.0, boost::math::constants::two_pi<double>());
		boost::variate_generator< boost::random::mt19937&, boost::random::uniform_real_distribution<double> > gen(rand_gen, uni_distr);
		for (unsigned int i = 0 ; i < circles.size(); i++)
		{
			double rot_angle = gen();
			Particle_circle::Vector_2 dir(std::cos(rot_angle), std::sin(rot_angle));
			double len = (gen()/10)*circles[i].radius();
			circles[i].translate(len*dir.x(), len*dir.y());
		}
	}
	void Particle_packer::shuffle()
	{
		if (!circles.empty())
		{
			std::vector<unsigned int> indices;
			indices.reserve(circles.size());
			for (unsigned int i = 0; i < circles.size(); i++)
				indices.push_back(i);
			std::random_shuffle(indices.begin(), indices.end());
			unsigned int shuffle_nb = 0.1*circles.size() + 1;
			std::vector<unsigned int> to_be_shuffled(indices.begin(), indices.begin()+shuffle_nb);

			std::random_shuffle(indices.begin(), indices.begin()+shuffle_nb);

				
			std::vector<Particle_circle> shuffled_circles;
			shuffled_circles.reserve(shuffle_nb);
			for (unsigned int i = 0; i < shuffle_nb; i++)
				shuffled_circles.push_back(circles[indices[i]]);

			//for (unsigned int i = 0; i < shuffle_nb; i++)
			//	std::cout<<indices[i]<<"->"<<to_be_shuffled[i]<<' ';
			//std::cout<<std::endl;

			for (unsigned int i = 0; i < shuffle_nb; i++)
			{
				Particle_circle::Vector_2 v(shuffled_circles[i].center(), circles[to_be_shuffled[i]].center());
				circles[to_be_shuffled[i]] = shuffled_circles[i];
				circles[to_be_shuffled[i]].translate(v.x(), v.y());
			}
		}
		else
		{
			std::vector<unsigned int> indices;
			indices.reserve(circle_unions.size());
			for (unsigned int i = 0; i < circle_unions.size(); i++)
				indices.push_back(i);
			std::random_shuffle(indices.begin(), indices.end());
			unsigned int shuffle_nb = 0.1*circle_unions.size() + 1;
			shuffle_nb = std::max<unsigned int>(shuffle_nb, 2); // guarantee one shuffle 
			std::vector<unsigned int> to_be_shuffled(indices.begin(), indices.begin()+shuffle_nb);

			std::random_shuffle(indices.begin(), indices.begin()+shuffle_nb);
			while (std::equal(to_be_shuffled.begin(), to_be_shuffled.end(), indices.begin()))
				std::random_shuffle(indices.begin(), indices.begin()+shuffle_nb);
	
			std::vector<Circle_union> shuffled_circle_unions;
			shuffled_circle_unions.reserve(shuffle_nb);
			for (unsigned int i = 0; i < shuffle_nb; i++)
				shuffled_circle_unions.push_back(circle_unions[indices[i]]);

			for (unsigned int i = 0; i < shuffle_nb; i++)
			{
				Particle_circle::Vector_2 v(shuffled_circle_unions[i].centroid(), circle_unions[to_be_shuffled[i]].centroid());
				circle_unions[to_be_shuffled[i]] = shuffled_circle_unions[i];
				circle_unions[to_be_shuffled[i]].transform(0.0, v.x(), v.y());
			}
		}

	}
	void Particle_packer::get_bbox(Geex::real& x_min, Geex::real& y_min, Geex::real& z_min, Geex::real& x_max, Geex::real& y_max, Geex::real& z_max)
	{
		x_min = rbox->x_min(); x_max = rbox->x_max();
		y_min = rbox->y_min(); y_max = rbox->y_max();
		z_min = rbox->z_min(); z_max = rbox->z_max();
	}
}
